//
//  Motion_Model.cpp
//  markerARLib
//
//  Created by fg on 6/20/16.
//  Copyright © 2016 CvidKal. All rights reserved.
//

#include "Motion_Model.hpp"


namespace markerAR
{
   
   RigidMotion Motion_Model::addPose(const Pose &now,double timestamp)
    {
        
        if (motionWindow.empty()) {
            RigidMotion back;
            back.pose = now;
            back.timestamp = timestamp;
            back.translationMotion.tv.setZero();
            back.translationMotion.ta.setZero();
            back.rotationMotion.rv.setZero();
            back.rotationMotion.ra.setZero();
            motionWindow.push(back);
            return back;
        }
        if (motionWindow.size() == windowSize) {
            motionWindow.pop();
        }
        
        RigidMotion& front = motionWindow.front();
        RigidMotion back;
        back.pose = now;
        back.timestamp = timestamp;
        back.translationMotion.tv = (back.pose.t - front.pose.t)/(back.timestamp -front.timestamp );
        back.translationMotion.ta = (back.translationMotion.tv - front.translationMotion.tv)/(back.timestamp-front.timestamp);
        back.rotationMotion.rv   = (back.pose.EulerAngle() - front.pose.EulerAngle())/(back.timestamp - front.timestamp);
        back.rotationMotion.ra = (back.rotationMotion.rv - front.rotationMotion.rv)/(back.timestamp - front.timestamp);
        motionWindow.push(back);
        return back;
    }
    
}